/*
  ZynAddSubFX - a software synthesizer
 
  AnalogFilter.C - Several analog filters (lowpass, highpass...)
  Copyright (C) 2002-2005 Nasca Octavian Paul
  Author: Nasca Octavian Paul

  This program is free software; you can redistribute it and/or modify
  it under the terms of version 2 of the GNU General Public License 
  as published by the Free Software Foundation.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License (version 2) for more details.

  You should have received a copy of the GNU General Public License (version 2)
  along with this program; if not, write to the Free Software Foundation,
  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA

*/
//#include "stdafx.h"
#include <math.h>
#include <stdio.h>
#include "AnalogFilter.h"

#define D_PI 6.283185f
#define PI 3.141598f
#define LOG_10 2.302585f
#define LOG_2  0.693147f
#define AMPLITUDE_INTERPOLATION_THRESHOLD 0.0001f
#define FF_MAX_VOWELS 6
#define FF_MAX_FORMANTS 12
#define FF_MAX_SEQUENCE 8
#define MAX_FILTER_STAGES 5
#define RND (rand()/(RAND_MAX+1.0))
#define float float
#define F2I(f,i) (i)=((f>0) ? ( (int)(f) ) :( (int)(f-1.0f) ))
#define dB2rap(dB) (float)((expf((dB)*LOG_10/20.0f)))
#define rap2dB(rap) (float)((20*log(rap)/LOG_10))
#define CLAMP(x, low, high)  (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x)))
#define INTERPOLATE_AMPLITUDE(a,b,x,size) ( (a) + ( (b) - (a) ) * (float)(x) / (float) (size) )
#define ABOVE_AMPLITUDE_THRESHOLD(a,b) ( ( 2.0f*fabs( (b) - (a) ) / ( fabs( (b) + (a) + 0.0000000001f) ) ) > AMPLITUDE_INTERPOLATION_THRESHOLD )
#define POLY 8
#define DENORMAL_GUARD 1e-18f	// Make it smaller until CPU problem re-appears
#define SwapFourBytes(data) ( (((data) >> 24) & 0x000000ff) | (((data) >> 8) & 0x0000ff00) | (((data) << 8) & 0x00ff0000) | (((data) << 24) & 0xff000000) )



AnalogFilter::AnalogFilter (unsigned char Ftype, REALTYPE Ffreq, REALTYPE Fq,
			    unsigned char Fstages)
{
  stages = Fstages;
  for (int i = 0; i < 3; ++i)
    {
      oldc[i] = 0.0;
      oldd[i] = 0.0;
      c[i] = 0.0;
      d[i] = 0.0;
    };
  type = Ftype;
  freq = Ffreq;
  q = Fq;
  gain = 1.0;
  if (stages >= MAX_FILTER_STAGES)
    stages = MAX_FILTER_STAGES;
  cleanup ();
  firsttime = 0;
  abovenq = 0;
  oldabovenq = 0;

};

AnalogFilter::~AnalogFilter ()
{
};

void
AnalogFilter::cleanup ()
{
  for (int i = 0; i < MAX_FILTER_STAGES + 1; ++i)
    {
      x[i].c1 = 0.0;
      x[i].c2 = 0.0;
      y[i].c1 = 0.0;
      y[i].c2 = 0.0;
      oldx[i] = x[i];
      oldy[i] = y[i];
    };
  needsinterpolation = 0;


};

void
AnalogFilter::computefiltercoefs ()
{
  REALTYPE tmp;
  REALTYPE omega, sn, cs, alpha, beta;
  int zerocoefs = 0;		//this is used if the freq is too high

  //do not allow frequencies bigger than samplerate/2
  REALTYPE freq = this->freq;
  if (freq > (SAMPLE_RATE / 2 - 500.0))
    {
      freq = (float)SAMPLE_RATE * .5f - 500.0f;
      zerocoefs = 1;
    };
  if (freq < 0.1)
    freq = 0.1f;
  //do not allow bogus Q
  if (q < 0.0)
    q = 0.0;
  REALTYPE tmpq, tmpgain;
  if (stages == 0)
    {
      tmpq = q;
      tmpgain = gain;
    }
  else
    {
      tmpq = (q > 1.0 ? powf (q, 1.0f /(float) (stages + 1)) : q);
      tmpgain = powf (gain, 1.0f / (float)(stages + 1));
    };

  //most of theese are implementations of 
  //the "Cookbook formulae for audio EQ" by Robert Bristow-Johnson
  //The original location of the Cookbook is:
  //http://www.harmony-central.com/Computer/Programming/Audio-EQ-Cookbook.txt
  switch (type)
    {
    case 0:			//LPF 1 pole         
      if (zerocoefs == 0)
	tmp = expf (-D_PI * freq / (float)SAMPLE_RATE);
      else
	tmp = 0.0f;
      c[0] = 1.0f - tmp;
      c[1] = 0.0;
      c[2] = 0.0;
      d[1] = tmp;
      d[2] = 0.0;
      order = 1;
      break;
    case 1:			//HPF 1 pole
      if (zerocoefs == 0)
	tmp = expf (-D_PI * freq / (float)SAMPLE_RATE);
      else
	tmp = 0.0f;
      c[0] = (1.0f + tmp) * .5f;
      c[1] = -(1.0f + tmp) * .5f;
      c[2] = 0.0f;
      d[1] = tmp;
      d[2] = 0.0f;
      order = 1;
      break;
    case 2:			//LPF 2 poles 
      if (zerocoefs == 0)
	{
	  omega = D_PI * freq / (float)SAMPLE_RATE;
	  sn = sinf (omega);
	  cs = cosf (omega);
	  alpha = sn / (2.0f * tmpq);
	  tmp = 1 + alpha;
	  c[0] = (1.0f - cs) * .5f / tmp;
	  c[1] = (1.0f - cs) / tmp;
	  c[2] = (1.0f - cs) * .5f / tmp;
	  d[1] = -2.0f * cs / tmp * (-1.0f);
	  d[2] = (1.0f - alpha) / tmp * (-1.0f);
	}
      else
	{
	  c[0] = 1.0;
	  c[1] = 0.0;
	  c[2] = 0.0;
	  d[1] = 0.0;
	  d[2] = 0.0;
	};
      order = 2;
      break;
    case 3:			//HPF 2 poles 
      if (zerocoefs == 0)
	{
	  omega =  D_PI * freq / (float)SAMPLE_RATE;
	  sn = sinf (omega);
	  cs = cosf (omega);
	  alpha = sn / (2.0f * tmpq);
	  tmp = 1.0f + alpha;
	  c[0] = (1.0f + cs) / 2.0f / tmp;
	  c[1] = -(1.0f + cs) / tmp;
	  c[2] = (1.0f + cs) / 2.0f / tmp;
	  d[1] = -2.0f * cs / tmp * (-1.0f);
	  d[2] = (1.0f - alpha) / tmp * (-1.0f);
	}
      else
	{
	  c[0] = 0.0;
	  c[1] = 0.0;
	  c[2] = 0.0;
	  d[1] = 0.0;
	  d[2] = 0.0;
	};
      order = 2;
      break;
    case 4:			//BPF 2 poles 
      if (zerocoefs == 0)
	{
	  omega = D_PI * freq / (float)SAMPLE_RATE;
	  sn = sinf (omega);
	  cs = cosf (omega);
	  alpha = sn / (2.0f * tmpq);
	  tmp = 1.0f + alpha;
	  c[0] = alpha / tmp * sqrtf (tmpq + 1.0f);
	  c[1] = 0.0f;
	  c[2] = -alpha / tmp * sqrtf (tmpq + 1.0f);
	  d[1] = -2.0f * cs / tmp * (-1.0f);
	  d[2] = (1.0f - alpha) / tmp * (-1.0f);
	}
      else
	{
	  c[0] = 0.0;
	  c[1] = 0.0;
	  c[2] = 0.0;
	  d[1] = 0.0;
	  d[2] = 0.0;
	};
      order = 2;
      break;
    case 5:			//NOTCH 2 poles 
      if (zerocoefs == 0)
	{
	  omega = D_PI * freq /(float) SAMPLE_RATE;
	  sn = sinf (omega);
	  cs = cosf (omega);
	  alpha = sn / (2.0f * sqrtf (tmpq));
	  tmp = 1.0f + alpha;
	  c[0] = 1.0f / tmp;
	  c[1] = -2.0f * cs / tmp;
	  c[2] = 1.0f / tmp;
	  d[1] = -2.0f * cs / tmp * (-1.0f);
	  d[2] = (1.0f - alpha) / tmp * (-1.0f);
	}
      else
	{
	  c[0] = 1.0;
	  c[1] = 0.0;
	  c[2] = 0.0;
	  d[1] = 0.0;
	  d[2] = 0.0;
	};
      order = 2;
      break;
    case 6:			//PEAK (2 poles)
      if (zerocoefs == 0)
	{
	  omega = D_PI * freq / (float)SAMPLE_RATE;
	  sn = sinf (omega);
	  cs = cosf (omega);
	  tmpq *= 3.0f;
	  alpha = sn / (2.0f * tmpq);
	  tmp = 1.0f + alpha / tmpgain;
	  c[0] = (1.0f + alpha * tmpgain) / tmp;
	  c[1] = (-2.0f * cs) / tmp;
	  c[2] = (1.0f - alpha * tmpgain) / tmp;
	  d[1] = -2.0f * cs / tmp * (-1.0f);
	  d[2] = (1.0f - alpha / tmpgain) / tmp * (-1.0f);
	}
      else
	{
	  c[0] = 1.0;
	  c[1] = 0.0;
	  c[2] = 0.0;
	  d[1] = 0.0;
	  d[2] = 0.0;
	};
      order = 2;
      break;
    case 7:			//Low Shelf - 2 poles
      if (zerocoefs == 0)
	{
	  omega = D_PI * freq / (float)SAMPLE_RATE;
	  sn = sinf (omega);
	  cs = cosf (omega);
	  tmpq = sqrtf (tmpq);
	  alpha = sn / (2.0f * tmpq);
	  beta = sqrtf (tmpgain) / tmpq;
	  tmp = (tmpgain + 1.0f) + (tmpgain - 1.0f) * cs + beta * sn;

	  c[0] =
	    tmpgain * ((tmpgain + 1.0f) - (tmpgain - 1.0f) * cs +
		       beta * sn) / tmp;
	  c[1] =
	    2.0f * tmpgain * ((tmpgain - 1.0f) - (tmpgain + 1.0f) * cs) / tmp;
	  c[2] =
	    tmpgain * ((tmpgain + 1.0f) - (tmpgain - 1.0f) * cs -
		       beta * sn) / tmp;
	  d[1] = -2.0f * ((tmpgain - 1.0f) + (tmpgain + 1.0f) * cs) / tmp * (-1.0f);
	  d[2] =
	    ((tmpgain + 1.0f) + (tmpgain - 1.0f) * cs - beta * sn) / tmp * (-1.0f);
	}
      else
	{
	  c[0] = tmpgain;
	  c[1] = 0.0;
	  c[2] = 0.0;
	  d[1] = 0.0;
	  d[2] = 0.0;
	};
      order = 2;
      break;
    case 8:			//High Shelf - 2 poles
      if (zerocoefs == 0)
	{
	  omega = D_PI * freq / (float)SAMPLE_RATE;
	  sn = sinf (omega);
	  cs = cosf (omega);
	  tmpq = sqrtf (tmpq);
	  alpha = sn / (2.0f * tmpq);
	  beta = sqrtf (tmpgain) / tmpq;
	  tmp = (tmpgain + 1.0f) - (tmpgain - 1.0f) * cs + beta * sn;

	  c[0] =
	    tmpgain * ((tmpgain + 1.0f) + (tmpgain - 1.0f) * cs +
		       beta * sn) / tmp;
	  c[1] =
	    -2.0f * tmpgain * ((tmpgain - 1.0f) + (tmpgain + 1.0f) * cs) / tmp;
	  c[2] =
	    tmpgain * ((tmpgain + 1.0f) + (tmpgain - 1.0f) * cs -
		       beta * sn) / tmp;
	  d[1] = 2.0f * ((tmpgain - 1.0f) - (tmpgain + 1.0f) * cs) / tmp * (-1.0f);
	  d[2] =
	    ((tmpgain + 1.0f) - (tmpgain - 1.0f) * cs - beta * sn) / tmp * (-1.0f);
	}
      else
	{
	  c[0] = 1.0;
	  c[1] = 0.0;
	  c[2] = 0.0;
	  d[1] = 0.0;
	  d[2] = 0.0;
	};
      order = 2;
      break;
    default:			//wrong type
      type = 0;
      computefiltercoefs ();
      break;
    };
};


void
AnalogFilter::setfreq (REALTYPE frequency)
{
  if (frequency < 0.1)
    frequency = 0.1f;
  REALTYPE rap = freq / frequency;
  if (rap < 1.0)
    rap = 1.0f / rap;

  oldabovenq = abovenq;
  abovenq = frequency > (SAMPLE_RATE / 2 - 500.0);

  int nyquistthresh = (abovenq ^ oldabovenq);


  if ((rap > 3.0) || (nyquistthresh != 0))
    {				//if the frequency is changed fast, it needs interpolation (now, filter and coeficients backup)
      for (int i = 0; i < 3; ++i)
	{
	  oldc[i] = c[i];
	  oldd[i] = d[i];
	};
      for (int i = 0; i < MAX_FILTER_STAGES + 1; ++i)
	{
	  oldx[i] = x[i];
	  oldy[i] = y[i];
	};
      if (firsttime == 0)
	needsinterpolation = 1;
    };
  freq = frequency;
  computefiltercoefs ();
  firsttime = 0;

};

void
AnalogFilter::setfreq_and_q (REALTYPE frequency, REALTYPE q_)
{
  q = q_;
  setfreq (frequency);
};

void
AnalogFilter::setq (REALTYPE q_)
{
  q = q_;
  computefiltercoefs ();
};

void
AnalogFilter::settype (int type_)
{
  type = type_;
  computefiltercoefs ();
};

void
AnalogFilter::setgain (REALTYPE dBgain)
{
  gain = dB2rap(dBgain);
  computefiltercoefs ();
};

void
AnalogFilter::setstages (int stages_)
{
  if (stages_ >= MAX_FILTER_STAGES)
    stages_ = MAX_FILTER_STAGES - 1;
  stages = stages_;
  cleanup ();
  computefiltercoefs ();
};

void
AnalogFilter::singlefilterout (REALTYPE * smp, fstage & x, fstage & y,
			       REALTYPE * c, REALTYPE * d)
{
  int i;
  REALTYPE y0;
  if (order == 1)
    {				//First order filter
      for (i = 0; i < PERIOD; ++i)
	{

	  y0 = smp[i] * c[0] + x.c1 * c[1] + y.c1 * d[1];
	  y.c1 = y0;
	  x.c1 = smp[i];
	  //output
	  smp[i] = y0 + DENORMAL_GUARD;
	};
    };
  if (order == 2)
    {				//Second order filter
      for (i = 0; i < PERIOD; ++i)
	{
	  y0 =
	    (smp[i] * c[0]) + (x.c1 * c[1]) + (x.c2 * c[2]) + (y.c1 * d[1]) +
	    (y.c2 * d[2]);
	  y.c2 = y.c1;
	  y.c1 = y0;
	  x.c2 = x.c1;
	  x.c1 = smp[i];
	  //output
	  smp[i] = y0 + DENORMAL_GUARD;
	};
    };
};

void
AnalogFilter::filterout (REALTYPE * smp)
{
  REALTYPE *ismp = NULL;	//used if it needs interpolation
  int i;
  if (needsinterpolation != 0)
    {
      ismp = new REALTYPE[PERIOD];
      for (i = 0; i < PERIOD; ++i)
	ismp[i] = smp[i];
      for (i = 0; i < stages + 1; ++i)
	singlefilterout (ismp, oldx[i], oldy[i], oldc, oldd);
    };

  for (i = 0; i < stages + 1; ++i)
    singlefilterout (smp, x[i], y[i], c, d);

  if (needsinterpolation != 0)
    {
      for (i = 0; i < PERIOD; ++i)
	{
	  REALTYPE x = (float) i / (float) PERIOD;
	  smp[i] = ismp[i] * (1.0f - x) + smp[i] * x;
	};
      delete (ismp);
      needsinterpolation = 0;
    };

};

REALTYPE AnalogFilter::H (REALTYPE freq)
{
  REALTYPE
    fr = freq / (float)SAMPLE_RATE * PI * 2.0f;
  REALTYPE
    x = c[0], y = 0.0;
  for (int n = 1; n < 3; ++n)
    {
      x += cosf ((float)n * fr) * c[n];
      y -= sinf ((float)n * fr) * c[n];
    };
  REALTYPE
    h = x * x + y * y;
  x = 1.0;
  y = 0.0;
  for (int n = 1; n < 3; ++n)
    {
      x -= cosf ((float)n * fr) * d[n];
      y += sinf ((float)n * fr) * d[n];
    };
  h = h / (x * x + y * y);
  return (powf (h, (float)(stages + 1) / 2.0f));
};


bool AnalogFilter::SetSampleRate(unsigned int iSamplePerSec)
{
	SAMPLE_RATE = iSamplePerSec;
	if(firsttime == 0)
	{
		setfreq_and_q (freq, q);
		firsttime = 1;
		d[0] = 0;			//this is not used
		outgain = 1.0;
	}
	return true;
}

bool AnalogFilter::SetBufferSize(int nSamples)
{
	if(nSamples<=0)
	{
		return false;
	}
	PERIOD=nSamples;
	return true;
}